/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-09-04 09:57:09
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-09-26 21:46:25
 */

#include "robotservofw.h"

uint8_t ServoCmd[256] = {0};
ServoTypeDef ServoOnlyOne = {0};
ServoTypeDef ServoAll[ServoNumber] = {0};
// 每个舵机的偏差值
int ServoBias[ServoNumber] = {22, -22, 27, 50, 27, -16};

/**
 * @function: Control_Servo_OnlyOne
 * @brief 打包控制单个舵机的命令
 * @param {uint8_t} *cmd：命令存储区
 * @param {ServoTypeDef} *servo：舵机结构体
 * @return {*}
 */
void Control_Servo_OnlyOne(uint8_t *cmd, ServoTypeDef *servo)
{
    if(servo->id > ServoID_MAX) servo->id = ServoID_MAX;
    if(servo->pwm > ServoPWM_MAX) servo->pwm = ServoPWM_MAX;
    else if(servo->pwm < ServoPWM_MIN) servo->pwm = ServoPWM_MIN;
    if(servo->time > ServoTIME_MAX) servo->time = ServoTIME_MAX;

    if(servo->id == Clamp_ID)
    {
        if(servo->pwm > Clamp_Max) servo->pwm = Clamp_Max;
        else if(servo->pwm < Clamp_Min) servo->pwm = Clamp_Min;
    }
    sprintf((char *)cmd, "#%03dP%04dT%04d!", servo->id, servo->pwm, servo->time);
}


/**
 * @function: Control_Servo_multi
 * @brief 打包控制多个舵机的命令
 * @param {uint8_t} *cmd：命令存储区
 * @param {ServoTypeDef} *servoAll：舵机结构体数组
 * @return {*}
 */
void Control_Servo_multi(uint8_t *cmd, ServoTypeDef *servoAll)
{
    uint8_t i = 0;
    uint8_t tempCmd[20] = {0};

    sprintf((char *)cmd, "{G0000");

    for(i=0; i<ServoNumber; i++)
    {
        if(servoAll->id > ServoID_MAX) servoAll->id = ServoID_MAX;
        if(servoAll->pwm > ServoPWM_MAX) servoAll->pwm = ServoPWM_MAX;
        else if(servoAll->pwm < ServoPWM_MIN) servoAll->pwm = ServoPWM_MIN;
        if(servoAll->time > ServoTIME_MAX) servoAll->time = ServoTIME_MAX;
        sprintf((char *)tempCmd, "#%03dP%04dT%04d!", servoAll->id, servoAll->pwm, servoAll->time);
        strcat((char *)cmd, (char *)tempCmd);
        servoAll++;
    }
    strcat((char *)cmd, "}");
}


/**
 * @function: Control_ServoReset
 * @brief: 打包舵机复位命令
 * @param {uint8_t} *cmd：命令存储区
 * @return {*}
 */
void Control_ServoReset(uint8_t *cmd)
{
    uint8_t i = 0;
    uint8_t tempCmd[20] = {0};

    sprintf((char *)cmd, "{G0001");

    for(i=0; i<ServoNumber; i++)
    {
        ServoAll[i].id = i;
        ServoAll[i].time = 1000;

        if(i == 3)
        {
            ServoAll[i].pwm = 900;
        }
        else
        {
            ServoAll[i].pwm = 1500;
        }
        Control_Servo_OnlyOne(tempCmd, &ServoAll[i]);
        strcat((char *)cmd, (char *)tempCmd);
        ServoAll[i].time = 150;
    }
    strcat((char *)cmd, "}");
}


/**
 * @function: CheckTime
 * @brief 检测时间是否超时了，单位mm
 * @param {uint32_t} old：旧的时间
 * @param {uint32_t} timerout：超时时间
 * @return {HAL_StatusTypeDef} 超时返回HAL_OK，没有超时返回HAL_ERROR
 */
static HAL_StatusTypeDef CheckTime(uint32_t old, uint32_t timerout)
{
    uint32_t nowTime = HAL_GetTick();
    int AfterTime = 0;
    if (nowTime < old)
    {
        AfterTime = HAL_MAX_DELAY - old + nowTime;
    }
    else
    {
        AfterTime = nowTime - old;
    }
    return (AfterTime > timerout) ? (HAL_OK) : (HAL_ERROR);
}


#if 1
/**
 * @function: Read_ServoPosition
 * @brief 读取舵机当前位置
 * @param {UART_HandleTypeDef} *huart：串口句柄
 * @param {uint8_t} servoID：舵机ID
 * @param {uint16_t} *position：存放位置的变量
 * @return {HAL_StatusTypeDef} HAL_OK：读取成功，HAL_ERROR：读取失败
 */
HAL_StatusTypeDef Read_ServoPosition(UART_HandleTypeDef *huart, uint8_t servoID, uint16_t *position)
{
    uint8_t cmd[20] = {0};
    uint8_t recv[20] = {0};
    uint8_t byte = 0, i = 0;
    uint32_t oldTiem = HAL_GetTick();

    sprintf((char*)cmd, "#%03dPRAD!", servoID);
    HAL_UART_Transmit(huart, cmd, strlen((char *)cmd), 100);

    while (CheckTime(oldTiem, 50) != HAL_OK)
    {
        if(HAL_UART_Receive(huart, &byte, 1, 1) == HAL_OK)
        {
            if((i == 0) && (byte == '#'))
            {
                recv[i++] = byte;
            }
            else if(i > 0)
            {
                recv[i++] = byte;
            }
            if(byte == '!')
            {
                uint8_t str[10];
                memcpy(str, recv+1, 3);
                if(atoi((const char*)str) != servoID)
                {
                    return HAL_ERROR;
                }
                if(recv[4] == 'P')
                {
                    memcpy(str, recv+5, 4);
                    *position = (uint16_t)atoi((const char*)str);
                    if(*position > ServoPWM_MIN)
                    {
                        return HAL_OK;
                    }
                }
                break;
            }
        }
    }
    if(DEBUG_PRINTF) printf("recv: %s\r\n", recv);
    return HAL_ERROR;
}
#else
/**
 * @function: Read_ServoPosition
 * @brief 读取舵机当前位置
 * @param {UART_HandleTypeDef} *huart：串口句柄
 * @param {uint8_t} servoID：舵机ID
 * @param {uint16_t} *position：存放位置的变量
 * @return {HAL_StatusTypeDef} HAL_OK：读取成功，HAL_ERROR：读取失败
 */
HAL_StatusTypeDef Read_ServoPosition(UART_HandleTypeDef *huart, uint8_t servoID, uint16_t *position)
{
    uint8_t cmd[20] = {0};
    uint8_t recv[20] = {0};
    uint8_t i = 0, j = 0;
    uint32_t oldTiem = HAL_GetTick();

    sprintf((char*)cmd, "#%03dPRAD!", servoID);
    // HAL_UART_Transmit(huart, cmd, strlen((char *)cmd), 100);
    DMA_ServoSendData(cmd, strlen((char *)cmd));

    while (CheckTime(oldTiem, 100) != HAL_OK)
    {
        if(Servo_DMA_Struct.Rx_flag)
        {
            Servo_DMA_Struct.Rx_flag = 0;
            //等待上一次的数据发送完毕
            while (Servo_DMA_Struct.Rx_len--)
            {
                if ((j == 0) && (Servo_DMA_Struct.Rx_Buff[i] == '#'))
                {
                    recv[j++] = Servo_DMA_Struct.Rx_Buff[i];
                }
                else if(j > 0)
                {
                    recv[j++] = Servo_DMA_Struct.Rx_Buff[i];
                    if(Servo_DMA_Struct.Rx_Buff[i] == '!')
                    {
                        uint8_t str[10];
                        memcpy(str, recv+1, 3);
                        if(atoi((const char*)str) != servoID)
                        {
                            return HAL_ERROR;
                        }
                        if(recv[4] == 'P')
                        {
                            memcpy(str, recv+5, 4);
                            *position = (uint16_t)atoi((const char*)str);
                            if(*position > ServoPWM_MIN)
                            {
                                return HAL_OK;
                            }
                        }
                        break;
                    }
                }
                i++;
            }
        }
    }
    if(DEBUG_PRINTF) printf("recv: %s\r\n", recv);
    return HAL_ERROR;
}
#endif


void Read_PositionAll(UART_HandleTypeDef *huart)
{
    uint8_t i = 0;
    uint16_t position;
    for (i = 0; i < ServoNumber; i++)
    {
        if(Read_ServoPosition(huart, i, &position) == HAL_OK)
        {
            // if(DEBUG_PRINTF) printf("Read ID:%d, Position: %d! \r\n", i, position);
            ServoAll[i].pwm = position;
        }
        else
        {
            // if(DEBUG_PRINTF) printf("Read ID:%d error! \r\n", i);
            break;
        }
    }

}


/**
 * @function: Control_Servo_PULK
 * @brief 设置单个舵机是否释放扭力
 * @param {uint8_t} *cmd：存储命令
 * @param {uint8_t} *cmd：存储命令
 * @param {uint8_t} state：1：不释放，0：释放
 * @return {*}
 */
void Control_Servo_PULR(uint8_t *cmd, uint8_t id, uint8_t state)
{
    if(state)
    {
        sprintf((char *)cmd, "#%03dPULR!", id);
    }
    else
    {
        sprintf((char *)cmd, "#%03dPULK!", id);
    }
}


/**
 * @function: Control_Servo_ALLPULR
 * @brief 设置所有舵机是否释放扭力
 * @param {uint8_t} *cmd：存储命令
 * @param {uint8_t} state：1：不释放，0：释放
 * @return {*}
 */
void Control_Servo_ALLPULR(uint8_t *cmd, uint8_t state)
{
    uint8_t i = 0;
    uint8_t tempCmd[20] = {0};

    sprintf((char *)cmd, "{G0001");

    for(i=0; i<ServoNumber; i++)
    {
        if(state)
        {
            sprintf((char *)tempCmd, "#%03dPULR!", i);
        }
        else
        {
            sprintf((char *)tempCmd, "#%03dPULK!", i);
        }
        strcat((char *)cmd, (char *)tempCmd);
    }

    strcat((char *)cmd, "}");
}


/**
 * @function: Servo_PWM2Angle
 * @brief 根据舵机PWM最大值和最小值、舵机当前模式最大角度，算出当前角度
 * @param {uint32_t} pwm：当前PWM
 * @return {double} 返回一个角度
 */
double Servo_PWM2Angle(uint32_t pwm)
{
    if(pwm > ServoPWM_MAX) pwm = ServoPWM_MAX;
    else if(pwm < ServoPWM_MIN) pwm = ServoPWM_MIN;
    return (pwm - ServoPWM_MIN) / (double)ServoOneAngle;
}












